#include "lidar_projection.h"

LidarProjection::LidarProjection(ros::NodeHandle& n)
{
    std::vector<double> camera0_vector_pose;
    n.getParam("camera0_pose",camera0_vector_pose);
    auto camera0_pose = init_pose(camera0_vector_pose);

    std::vector<double> camera1_vector_pose;
    n.getParam("camera1_pose",camera1_vector_pose);
    auto camera1_pose = init_pose(camera1_vector_pose);

    std::string camera0_name = "camera0";
    std::string camera1_name = "camera1";

    std::shared_ptr<CameraProjection> projection0_ptr(new CameraProjection(camera0_pose,camera0_name,true));
    std::shared_ptr<CameraProjection> projection1_ptr(new CameraProjection(camera1_pose,camera1_name,true));

    camera_projection_ptr_vec_.emplace_back(projection0_ptr);
    camera_projection_ptr_vec_.emplace_back(projection1_ptr);

    auto image0_sub = n.subscribe("/camera0/image_raw",1,&CameraProjection::image_callback,projection0_ptr.get());
    auto camerainfo0_sub = n.subscribe("/camera0/camera_info",1,&CameraProjection::camerainfo_callback,projection0_ptr.get());

    auto image1_sub = n.subscribe("/camera1/image_raw",1,&CameraProjection::image_callback,projection1_ptr.get());
    auto camerainfo1_sub = n.subscribe("/camera1/camera_info",1,&CameraProjection::camerainfo_callback,projection1_ptr.get());    // auto image1_sub = n.subscribe("/camera")
    
    image_sub_vec.emplace_back(image0_sub);
    image_sub_vec.emplace_back(image1_sub);
    camerainfo_sub_vec.emplace_back(camerainfo0_sub);
    camerainfo_sub_vec.emplace_back(camerainfo1_sub);

    detect_client = n.serviceClient<migratable_rl::detect>("/yolo_detect");
    semantics_lidar_pub = n.advertise<migratable_rl::semantics_lidar>("/semantics_scan",5);
    semantics_lidar_visualize_pub = n.advertise<visualization_msgs::Marker>("/semantics_scan_vis",5);

    for(auto& ptr:camera_projection_ptr_vec_)
    {
        ptr->set_detect_client(detect_client);
    }
    lidar_sub = n.subscribe("/scan",100,&LidarProjection::lidar_callback,this);
    std::vector<double> lidar_vector_pose;
    n.getParam("lidar_pose",lidar_vector_pose);
    lidar_pose_ = init_pose(lidar_vector_pose);
}

Eigen::Matrix4d LidarProjection::init_pose(std::vector<double> vector_pose)
{
    assert(vector_pose.size() == 6);
    auto rotation_matrix3 = Eigen::AngleAxisd(vector_pose[5], Eigen::Vector3d::UnitZ()) * 
                                                      Eigen::AngleAxisd(vector_pose[4], Eigen::Vector3d::UnitY()) * 
                                                      Eigen::AngleAxisd(vector_pose[3], Eigen::Vector3d::UnitX());  

    Eigen::Matrix4d matrix = Eigen::Matrix4d::Identity();
    matrix.block<3,3>(0,0) = rotation_matrix3.matrix();
    matrix(0,3) = vector_pose[0];
    matrix(1,3) = vector_pose[1];
    matrix(2,3) = vector_pose[2];
    std::cout<<matrix<<std::endl;
    return matrix;
}

Eigen::Vector3d LidarProjection::transform_point(double r,double theta)
{
    double x = r*cos(theta);
    double y = r*sin(theta);
    Eigen::Vector4d lidar_point(x,y,0,1);
    auto point_in_camera = lidar_pose_.inverse()*lidar_point;
    return point_in_camera.head(3);
}

void LidarProjection::visulize_semantics_lidar(migratable_rl::semantics_lidar& semantics_lidar)
{
    visualization_msgs::Marker points;
    points.header.frame_id = "/base_scan";
    points.header.stamp = ros::Time::now();
    points.action = visualization_msgs::Marker::ADD;
    points.pose.orientation.w = 1.0;
    points.type = visualization_msgs::Marker::POINTS;
    points.color.g = 1.0f;
    points.color.a = 1.0;
    points.scale.x = 0.1;
    points.scale.y = 0.1;

    auto& lidar = semantics_lidar.lidar_point;

    for(int i=0;i<lidar.ranges.size();i++)
    {
        if(semantics_lidar.type[i] == -1)
            continue;
        double r = lidar.ranges[i];
        double theta = lidar.angle_min+i*lidar.angle_increment;
        double x = r*cos(theta);
        double y = r*sin(theta);

        geometry_msgs::Point p;
        p.x = x;
        p.y = y;
        p.z = 0;
        points.points.emplace_back(p);
    }
    semantics_lidar_visualize_pub.publish(points);
}


void LidarProjection::lidar_callback(const sensor_msgs::LaserScanConstPtr& msg)
{
    std::vector<Eigen::Vector3d> point_in_base;
    for(int i=0;i<msg->ranges.size();i++)
    {
        double r = msg->ranges[i];
        double theta = msg->angle_min+i*msg->angle_increment;
        auto in_base = transform_point(r,theta);
        point_in_base.emplace_back(in_base);
    }

    migratable_rl::semantics_lidar semantics_lidar_point;
    semantics_lidar_point.lidar_point = *msg;
    for(auto& ptr:camera_projection_ptr_vec_)   //重置已生成的雷达点
        ptr->reset_vector();
    for(int i=0;i<msg->ranges.size();i++)
    {
        bool semantics = false;
        for(auto& ptr:camera_projection_ptr_vec_)
        {
            int idx = ptr->point_projection(point_in_base[i]);
            if(idx != -1)
            {
                semantics = true;
                semantics_lidar_point.type.emplace_back(idx);
                break;
            }
        }
        if(!semantics)
            semantics_lidar_point.type.emplace_back(-1);
    }
    assert(semantics_lidar_point.lidar_point.ranges.size() == semantics_lidar_point.type.size());
    semantics_lidar_pub.publish(semantics_lidar_point);
    visulize_semantics_lidar(semantics_lidar_point);
    cv::waitKey(1);

}


int main(int argc,char** argv)
{
    ros::init(argc,argv,"lidar_projection");
    ros::NodeHandle n("~");
    auto lidar_projection =  LidarProjection(n);
    ros::spin();
}